
#include "rviz_custom_plugins/panels/waypoint_panel.h"
#include "rviz_custom_plugins/utils/interactive_marker_manager.h"
#include <QString>
#include <cstddef>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <QStringBuilder>
#include <rviz_common/logging.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_util.h"
#include "rviz_custom_plugins/utils/interactive_marker_factory.h"
#include "rviz_custom_plugins/utils/project_manager.h"
#include "rviz_custom_plugins/models/poses.h"
#include <QDir>
#include <QFile>
#include <QTextStream>
#include <nlohmann/json.hpp>


using nlohmann::json;

using rviz_custom_plugins::utils::InteractiveMarkerManager;
using rviz_custom_plugins::utils::getMetadataItem;
using rviz_custom_plugins::utils::InteractiveMarkerFactory;


using rviz_custom_plugins::utils::MARKER_ADDED;
using rviz_custom_plugins::utils::MARKER_UPDATED;
using rviz_custom_plugins::utils::MARKER_REMOVED;

using rviz_custom_plugins::utils::ProjectManager;
using rviz_custom_plugins::models::Poses;
using rviz_custom_plugins::models::Pose;


namespace rviz_custom_plugins
{

namespace panels
{

WaypointPanel::WaypointPanel(QWidget *parent) :
    rviz_common::Panel(parent), ui(new Ui::waypointPanel())
{
    ui->setupUi(this);
    ProjectManager* projectManager = ProjectManager::getInstance();
    tableModel = new InteractiveMarkerModel();
    ui->tableWaypoints->setModel(tableModel);
    // ui->tableWaypoints->setColumnWidth(0, 50);
    // ui->tableWaypoints->setColumnHidden(0, true);
    ui->tableWaypoints->setSelectionMode(QAbstractItemView::SingleSelection);
    ui->tableWaypoints->setSelectionBehavior(QAbstractItemView::SelectRows);
    // 设置列自动拉伸
    ui->tableWaypoints->horizontalHeader()->setSectionResizeMode(0, QHeaderView::Stretch);
    ui->tableWaypoints->horizontalHeader()->setSectionResizeMode(1, QHeaderView::Stretch);
    ui->tableWaypoints->horizontalHeader()->setSectionResizeMode(2, QHeaderView::Stretch);
    ui->tableWaypoints->horizontalHeader()->setSectionResizeMode(3, QHeaderView::Stretch);
    ui->tableWaypoints->show();

    ui->tableWaypoints->setContextMenuPolicy(Qt::CustomContextMenu);

    qRegisterMetaType<interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr>(
       "interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr"
    );
	connect(ui->btnAdd, &QPushButton::clicked, this, &WaypointPanel::addButtonClicked);
    connect(ui->btnInsert, &QPushButton::clicked, this, &WaypointPanel::insertButtonClicked);
    connect(ui->btnRemove, &QPushButton::clicked, this, &WaypointPanel::removeButtonClicked);
    // connect(this, &WaypointPanel::markerFeedbackReceived,  this, &WaypointPanel::handleMarkerFeedback, Qt::QueuedConnection);
    connect(tableModel, &InteractiveMarkerModel::markerOrderUpdated, this, &WaypointPanel::handlerMarkersReorder, Qt::DirectConnection);

    connect(projectManager, &ProjectManager::projectCreated, this, &WaypointPanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectOpened, this, &WaypointPanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectSaved, this, &WaypointPanel::handleProjectSaved, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectClosed, this, &WaypointPanel::handleProjectClosed, Qt::DirectConnection);

}


void WaypointPanel::load(const Config& config)
{
    Panel::load(config);
	// set marker server namespace
    QString nameSpace;
    config.mapGetString("NameSpace", &nameSpace);
	markerThread = InteractiveMarkerManager::getInstance()->addThread(nameSpace);
    connect(markerThread.get(), &InteractiveMarkerThread::interactiveMarkerFeedback, this, &WaypointPanel::handleMarkerFeedback, Qt::QueuedConnection);
    connect(markerThread.get(), &InteractiveMarkerThread::activeStatusChanged, this, &WaypointPanel::handleActiveStatusChanged, Qt::QueuedConnection);
    handleActiveStatusChanged(markerThread->getActive());
}

void WaypointPanel::addButtonClicked()
{
    Pose pose;
    addItem(tableModel->rowCount(), pose);
}

void WaypointPanel::addItem(int row, Pose pose)
{
    InteractiveMarker intMarker = InteractiveMarkerFactory::createWaypoint(pose);

    markerThread->insert(intMarker);
    tableModel->insertMarker(row, intMarker);
}

void WaypointPanel::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback) {

    // std::ostringstream sstm;
    // sstm << " feedback->marker_name:" << feedback->marker_name;
    // RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

    // 处理新增marker
    if(MARKER_ADDED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "add marker marker_name:" << feedback->marker_name;
        // sstm << " marker_row:" << markerRow;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
    }
    else if(MARKER_UPDATED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "update marker marker_name:" << feedback->marker_name;
        // sstm << " marker_row:" << markerRow;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
    }
    else if(MARKER_REMOVED == feedback->client_id)
    {
        QString markerName = QString::fromStdString(feedback->marker_name);
        // int markerRow = tableModel->getMarkerRow(markerName);
        // tableModel->removeMarker(markerRow);

        std::ostringstream sstm;
        sstm << "remove marker marker_name:" << feedback->marker_name;
        // sstm << " marker_row:" << markerRow;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

        return;
    }


    switch (feedback->event_type) {
        case visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE:
        {
            QString markerName = QString::fromStdString(feedback->marker_name);
            int row = tableModel->getMarkerRow(markerName);
            QModelIndex index = tableModel->index(row, 0); // 获取行首单元格索引
            ui->tableWaypoints->setCurrentIndex(index);
            tableModel->handleMarkerFeedback(feedback);
            break;
        }
    }
}

void WaypointPanel::removeButtonClicked()
{
    QModelIndex currentIndex = ui->tableWaypoints->selectionModel()->currentIndex();
    if(currentIndex.isValid()) {
        int currentRow = currentIndex.row(); // 当前行号
        removeItem(currentRow);
    }
}


void WaypointPanel::removeItem(int row)
{
    InteractiveMarker intMarker = tableModel->getRowMarker(row);
    markerThread->erase(intMarker.name);

    QString markerName = QString::fromStdString(intMarker.name);
    int markerRow = tableModel->getMarkerRow(markerName);
    tableModel->removeMarker(markerRow);
}


void WaypointPanel::insertButtonClicked() {
    QModelIndex currentIndex = ui->tableWaypoints->selectionModel()->currentIndex();
    if(currentIndex.isValid()) {
        int currentRow = currentIndex.row();

        Pose pose;
        addItem(currentRow, pose);
    }
}


void WaypointPanel::handlerMarkersReorder(int row, std::list<InteractiveMarker>& markers) {
    auto it = markers.begin();
    std::advance(it, row); // 移动到指定索引

    for ( ;it != markers.end();++it) {
        int markerShowRow = getMetadataItem(*it, ROW, "-1").toInt() + 1;
        it->description = std::to_string(markerShowRow);
        markerThread->insert(*it);
    }
}

void WaypointPanel::handleActiveStatusChanged(bool active) {
    if (active) {
        setEnabled(true);
        parentWidget()->show();
    } else {
        setEnabled(false);
        parentWidget()->hide();
    }
}



void WaypointPanel::handleProjectLoaded(QSharedPointer<QVariantMap> projectData)
{
    QVariant waypointsVar = (*projectData)["waypoints"];
    Poses waypoints = waypointsVar.value<Poses>();
    for (const auto& waypoint : waypoints.poses)
    {
        addItem(waypoint.id - 1, waypoint);
    }
}


void WaypointPanel::handleProjectSaved(QSharedPointer<QVariantMap> projectData)
{
    auto markers = tableModel->getMarkers();
    // std::ostringstream sstm;
    // sstm << " markers size:" << markers.size();
    // RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

    Poses waypoints;

    int index = 1;
    for (auto it = markers.begin(); it != markers.end(); it++, ++index) 
    {
        const auto& pos = it->pose.position;
        Pose pose;
        pose.id = index;
        pose.position = it->pose.position;
        pose.orientation = it->pose.orientation;
        waypoints.poses.push_back(pose);
    }

    (*projectData)["waypoints"] = QVariant::fromValue(waypoints);
    
}


void WaypointPanel::handleProjectClosed(QSharedPointer<QVariantMap> projectData)
{
    int lastRow = tableModel->rowCount() - 1;
    for(;lastRow >= 0; lastRow--)
    {
        removeItem(lastRow);
    }
} 

}

}



#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::panels::WaypointPanel, rviz_common::Panel )
